#include "module.h"

#include "model.h"
#include "renderer/primitives.h"
#include "renderer/renderer.h"

#define POSITION_ATTRIB 0
#define VERTEX_NORMAL_ATTRIB 1

Module::Module(int w, int h):PhysicsObject(),Pickable(){
    w >= 1 ? width = w : width = 1;
    h >= 1 ? height = h : height = 1;

    links.push_back(Link( 0, 0,-1));
    links.push_back(Link( 1, 0, 0));
    links.push_back(Link( 0, 1, 0));
    links.push_back(Link( 0, 0, 1));
    links.push_back(Link(-1, 0, 0));
    links.push_back(Link( 0,-1, 0));



//    const float numVerticesInData = 3;
//    float data[3][3] = {
//                           { 0.0, 1.0, 0.0   },
//                           {-1.0,-1.0, 0.0  },
//                           { 1.0,-1.0, 0.0  }
//                       };
}

void Module::Draw(){
    float angle;
    Math::Vector3 axis;
    rotate+=1;

    if(Math::ParallelVector(heading, Math::k())){
       angle = 0;
    }
    else{
        angle = Math::AngleBetween(heading, Math::k());
        axis = Math::CrossProduct(heading, Math::k());
    }

    glPushMatrix();

        glTranslatef(position.x, position.y, position.z);
        glRotatef(angle, heading.x, heading.y, heading.z);
        for(auto link:links){
            link.Draw();
        }
        glColor3f(0.5, 0.5, 0.5);
        Primitives::DrawCube(0.8);
//
//        Primitives::DrawArrow(heading);
//        glBindVertexArray(modelVAO);
//        glEnableVertexAttribArray(0);
//        glEnableVertexAttribArray(1);
//        glDrawArrays(GL_TRIANGLES, 0, model->VertexCount());
    //    std::cout<<"error: "<<glGetError()<<std::endl;

    glPopMatrix();
}

void Module::Pick(){
    float angle = Math::AngleBetween(heading, Math::Vector3(0, 1, 0));
    glPushMatrix();
    glRotatef(angle, heading.x, heading.y, heading.z);
    glTranslatef(position.x, position.y, position.z);
    glColor3f((float)r()/256, (float)g()/256, (float)b()/256);
    Primitives::DrawCube(0.8);

    for(auto link:links){
        link.Pick();
    }
    glPopMatrix();
}

void Module::Update(int dt){
    PhysicsObject::Update(dt);
}

Link* Module::CheckLinkID(unsigned int id){
    for(unsigned int i=0;i<links.size();i++){
        if(links.at(i).CheckID(id)){
            return &links.at(i);
        }
    }
    return nullptr;
}

void Module::RotateToNewHeading(Math::Vector3 axis, float angle){
    heading = Math::RotateVector(heading, axis, angle);
    for(auto link:links){
        link.RotateToNewHeading(axis, angle);
    }
}

void Module::AttachModule(Module* newModule, Link* parentLink){
    Math::Vector3 newPos;
    Link* childLink = newModule->GetBaseLink();
    Math::Vector3 parentHeading = parentLink->Heading();
    Math::Vector3 childLinkHeading = Math::ScalarProduct(childLink->Heading(),-1);    // We want the vectors to be opposed
    float angle = Math::AngleBetween(parentHeading, childLinkHeading);
    Math::Vector3 axis = Math::CrossProduct(parentHeading, childLinkHeading);

    if(Math::ParallelVector(parentHeading, childLinkHeading)){
        // We're good
    }
    else if(Math::OpposedVector(parentHeading, childLinkHeading)){
        Math::Vector3 newHeading = Math::ScalarProduct(newModule->Heading(), -1);
        newModule->SetHeading(newHeading);
    }
    else{
        newModule->RotateToNewHeading(axis, angle);
    }

    newPos = Math::VectorAddition3f(this->position, Math::ScalarProduct(parentHeading, 2));
    newModule->SetPosition(newPos);
}
